#pragma once
#include <vector>
#include <opencv2/core/core.hpp>
#include <math.h>
#include "Segment.h";
#include "NLSSolver.h";
#include "SegmentHelper.h"

using namespace std;
using namespace cv;


class G1SegmentsFitting:public NLSSolver
{
protected:
	Mat _observedData;
	vector<Segment*> _segments;
	
	double getResidual(const Mat &params, int &residualIndex)
	{
		int segmentIndex = 0;
		int processedParamIndex =0;
		while(true)
		{
			int numOfParamAtSegment=0;
			if(_segments.at(segmentIndex)->type == SEGTYPE_LINE) numOfParamAtSegment = 4;
			else if(_segments.at(segmentIndex)->type== SEGTYPE_CIRC) numOfParamAtSegment = 6;

			if(processedParamIndex + numOfParamAtSegment -1 >= residualIndex){
				break;
			}
			else{
				segmentIndex ++;//next segment
				processedParamIndex += numOfParamAtSegment;
			}
		}

		//starting point and angle of this segment
		int localParamIndex = residualIndex - processedParamIndex;
		Mat fullParams = convertToFullParams(params, _segments);
		Point2d startingPoint = getStartingPoint(fullParams, _segments, 0, segmentIndex);
		double startingAngle = getStartingAngle(fullParams, _segments, 0, segmentIndex);

		switch (_segments.at(segmentIndex)->type)
		{
		case SEGTYPE_LINE:
			if(localParamIndex == 0)
			{
				return _observedData.at<double>(residualIndex, 0) - startingPoint.x;
			}
			else if(localParamIndex == 1)
			{
				return _observedData.at<double>(residualIndex, 0) - startingPoint.y;
			}
			else if(localParamIndex == 2)
			{
				double length = fullParams.at<double>(segmentIndex, 3);
				double endingPointX = startingPoint.x + length*cos(startingAngle);
				return  _observedData.at<double>(residualIndex, 0) - endingPointX;
			}
			else if(localParamIndex == 3)
			{
				double length = fullParams.at<double>(segmentIndex, 3);
				double endingPointY = startingPoint.y + length*sin(startingAngle);
				return  _observedData.at<double>(residualIndex, 0) - endingPointY;
			}
			break;
		case SEGTYPE_CIRC:
			double endingAngle = fullParams.at<double>(segmentIndex, 3);
			double sign = 1;//rotate direction  + non clockwise
			if(endingAngle < startingAngle) sign = -1; // clockwise

			if(localParamIndex == 0)//residual for center point
			{
				double radiusOfCircle = fullParams.at<double>(segmentIndex, 4);
				double centerPointX = startingPoint.x - radiusOfCircle*cos(startingAngle - sign*PI/2);
				return _observedData.at<double>(residualIndex, 0) - centerPointX;
			}
			else if(localParamIndex == 1)
			{
				double radiusOfCircle = fullParams.at<double>(segmentIndex, 4);
				double centerPointY = startingPoint.y - radiusOfCircle*sin(startingAngle - sign*PI/2);
				return _observedData.at<double>(residualIndex, 0) - centerPointY;
			}
			else if(localParamIndex == 2)
			{
				return _observedData.at<double>(residualIndex, 0) - startingPoint.x;
			}
			else if(localParamIndex == 3)
			{
				return _observedData.at<double>(residualIndex, 0) - startingPoint.y;
			}
			else if(localParamIndex == 4)
			{
				double radiusOfCircle = fullParams.at<double>(segmentIndex, 4);

				double centerPointX = startingPoint.x - radiusOfCircle*cos(startingAngle - sign*PI/2);
				double endingPointX = centerPointX + radiusOfCircle*cos(endingAngle - sign*PI/2);

				return _observedData.at<double>(residualIndex, 0) - endingPointX;
			}
			else if(localParamIndex == 5)
			{
				double radiusOfCircle = fullParams.at<double>(segmentIndex, 4);

				double centerPointY = startingPoint.y - radiusOfCircle*sin(startingAngle - sign*PI/2);
				double endingPointY = centerPointY + radiusOfCircle*sin(endingAngle - sign*PI/2);

				return _observedData.at<double>(residualIndex, 0) - endingPointY;
			}
			break;
		}
	}
	
	double getDerivative(const Mat &params, int &residualIndex, int &paramIndex)
	{
		// Assumes input is a single row matrix
		int sizeOfPrams = params.rows;
		// Returns the derivative of the nth parameter
		Mat params1 = params.clone();
		Mat params2 = params.clone();

		// Use central difference  to get derivative
		params1.at<double>(paramIndex, 0) -= DERIV_STEP;
		params2.at<double>(paramIndex, 0) += DERIV_STEP;

		//Mat fullParams1 = convertToFullParams(params1, _segments);
		//Mat fullParams2 = convertToFullParams(params2, _segments);

		//printf("params1 \n");
		//printMatrix(params1);
		//printf("params2 \n");
		//printMatrix(params2);
		//printf("fullParams1 \n");
		//printMatrix(fullParams1);
		//printf("fullParams2 \n");
		//printMatrix(fullParams2);

		double p1 = getResidual(params1, residualIndex);
		double p2 = getResidual(params2, residualIndex);

		double d = (p2 - p1) / (2*DERIV_STEP);
		return d;
	}

	Mat convertToFullParams(const Mat params, vector<Segment*> segments)
	{
		Mat fullParams = Mat(segments.size(), 5, CV_64F);
	
		Point2d lastEndingPoint = Point2d(params.at<double>(0,0), params.at<double>(1,0));
		double lastEndingAngle = params.at<double>(2,0);
		int indexOfParams = 2;
		for(int i=0;i< segments.size(); i++ )
		{
			fullParams.at<double>(i, 0) = lastEndingPoint.x;
			fullParams.at<double>(i, 1) = lastEndingPoint.y;
			fullParams.at<double>(i, 2) = lastEndingAngle;

			if (segments.at(i)->type == SEGTYPE_LINE)
			{
				Segment* s = segments.at(i);
			
				indexOfParams++;
				double length = params.at<double>(indexOfParams, 0);
				fullParams.at<double>(i, 3) = length; 

				lastEndingPoint += Point2d(length *cos(lastEndingAngle), length *sin(lastEndingAngle));
			}
			else if(segments.at(i)->type == SEGTYPE_CIRC)
			{
				indexOfParams++;
				double endingAngle = params.at<double>(indexOfParams, 0); 
				fullParams.at<double>(i, 3) = endingAngle;

				double sign = 1;
				if(endingAngle < lastEndingAngle) sign = -1;


				indexOfParams++;
				double radius =  params.at<double>(indexOfParams, 0); 
				fullParams.at<double>(i, 4) = radius; 

				Point2d center = Point2d( lastEndingPoint.x - radius*cos(lastEndingAngle - sign*PI/2), lastEndingPoint.y - radius*sin(lastEndingAngle- sign*PI/2));
				lastEndingPoint = center + Point2d(radius*cos(endingAngle- sign*PI/2),radius*sin(endingAngle- sign*PI/2));
				lastEndingAngle = endingAngle;
			}
		}
		return fullParams;
	}

	double getStartingAngle(Mat fullParams, const vector<Segment*> segments, const int originSegmentIndex, int segmentIndex) const 
	{
		if(segmentIndex == originSegmentIndex)
		{
			return fullParams.at<double>(0,2);
		}
		else
		{
			switch (segments.at(segmentIndex-1)->type)
			{
			case SEGTYPE_LINE:
				return getStartingAngle(fullParams, segments, originSegmentIndex, segmentIndex -1);
				break;
			case SEGTYPE_CIRC:
				return fullParams.at<double>(segmentIndex-1, 3);
				break;
			}
		}
	}

	Point2d getStartingPoint(Mat fullParams, const vector<Segment*> segments, const int originSegmentIndex, int segmentIndex) const 
	{
		if(segmentIndex == originSegmentIndex)
		{
			return Point2d(fullParams.at<double>(0,0),fullParams.at<double>(0,1));
		}
		else
		{
			if(segments.at(segmentIndex-1)->type == SEGTYPE_LINE){
				Point2d startingPointOfPrevSegment = getStartingPoint(fullParams, segments, originSegmentIndex, segmentIndex -1);
				double angleOfPrevSegment = getStartingAngle(fullParams, segments, originSegmentIndex, segmentIndex -1);
				double len = fullParams.at<double>(segmentIndex-1, 3);
				return Point2d(startingPointOfPrevSegment.x + len*cos(angleOfPrevSegment), startingPointOfPrevSegment.y + len*sin(angleOfPrevSegment));
			}
			else if(segments.at(segmentIndex-1)->type == SEGTYPE_CIRC){ 
				Point2d startingPointOfPrevSegment = getStartingPoint(fullParams, segments, originSegmentIndex, segmentIndex -1);
				double startingAngleOfCircle = getStartingAngle(fullParams, segments, originSegmentIndex, segmentIndex -1);
			
				double endingAngleOfCircle = fullParams.at<double>(segmentIndex-1, 3);
				double radiusOfCircle = fullParams.at<double>(segmentIndex-1, 4);

				double sign = 1;//rotate direction  + non clockwise
				if(endingAngleOfCircle < startingAngleOfCircle) sign = -1; // clockwise

				Point2d	center = Point2d(startingPointOfPrevSegment.x - radiusOfCircle*cos(startingAngleOfCircle - sign*PI/2), startingPointOfPrevSegment.y - radiusOfCircle*sin(startingAngleOfCircle - sign*PI/2));
				return Point2d(center.x + radiusOfCircle*cos(endingAngleOfCircle-sign*PI/2), center.y + radiusOfCircle*sin(endingAngleOfCircle-sign*PI/2));
			}
		}
	}

public:
	G1SegmentsFitting(const Mat &observedData,vector<Segment*> &segments)
	{
		_observedData= observedData;
		_segments = segments;
	}

	Mat solveByGaussNewton(Mat startingParams)
	{
		return GaussNewton(_observedData.rows, startingParams);
	}
	
};